#include <QDebug>
#include <qmath.h>

#include "vehicle.h"
#include "world.h"
#include "utilities.h"

using namespace Utilities;

Vehicle::Vehicle(const QString &name, VehicleType *type)
{
  mName = name;
  mType = type;
  mDragConstant = 0.3f;
  mWheelBase = 3.5f;
  mBrakeVal = 0.0f;
  mAccelVal = 0.0f;
  mDriver = NULL;
  mBraking = false;
  mPrevAccelerate = 0.0f;
  mGoalSpeed = 0.0f;
  setDirection(0.0f);
  setMass(1000.0f);
  setMaxEngineForce(9800);
  mOdometer = 0;
  qDebug() << "VEHICLE: spawned> " << mName;
}

qreal Vehicle::timeUntilSpeed(const qreal speedMPH)
{
  qreal speedDiff = (speed(true)-speedMPH) * MPH_TO_MPS;
  return ( (speedDiff / (mMaxAcceleration))*mMass );
}

// This does not account for drag
qreal Vehicle::distUntilSpeed(const qreal speedMPH)
{
  // Below is did not work
  //qreal dist = ( mVelocity.magnitude() * mVelocity.magnitude() / (2.0 * mDragConstant * GRAVITY_MPS2) );
  // Don't know why I'm missing 3 ticks, need that 3.
  qreal tus = timeUntilSpeed(speedMPH);
  return ( 0.50 * mVelocity.magnitude() * (tus-(World::gSimTimeStep*3)) ) * FEET_PER_METER;
}
void Vehicle::setName(const QString &value)
{
  mName = value;
}

QString Vehicle::name()
{
  return mName;
}

void Vehicle::setType(VehicleType *type)
{
  mType = type;
}

VehicleType* Vehicle::type()
{
  return mType;
}

void Vehicle::setAcceleration(const Point &value)
{
  mAcceleration = value;
  //qDebug() << "VEHICLE: setAcceleration> val=" << value.toString() << " acl=" << mAcceleration.toString();
}

Point Vehicle::acceleration()
{
  return mAcceleration;
}

void Vehicle::setVelocity(const Point &value)
{
  if(value.magnitude() < .001)
    mVelocity = Point(0,0);
  else
    mVelocity = value;
}

Point Vehicle::velocity()
{
  return mVelocity;
}

void Vehicle::setPosition(const Point &value)
{
  mPosition = value;
  //qDebug() << "VEHICLE: setPosition> val=" << value.toString();
}

Point Vehicle::position()
{
  return mPosition;
}

void Vehicle::setDirection(const qreal angle)
{
  qreal newVal=angle;
  Point::clampAngleTo180(newVal);
  newVal *= (M_PI/180.0); // DEG_TO_RAD
  mDirection.setX(qCos(newVal));
  mDirection.setY(qSin(newVal));
  //qDebug() << "VEHICLE: setDirection> val=" << newVal << " dir=" << mDirection.toString();
}
/*
qreal Vehicle::direction()
{
  return mDirection.angle();
}
*/

void Vehicle::setDirection(const Point &value)
{
  mDirection = value.normalized();
}

Point Vehicle::direction()
{
  return mDirection;
}

void Vehicle::setMaxTurnRate(const qreal value)
{
  mMaxTurnRate = value;
}

qreal Vehicle::maxTurnRate()
{
  return mMaxTurnRate;
}

void Vehicle::setMaxAcceleration(const qreal value)
{
  mMaxAcceleration = value;
}

qreal Vehicle::maxBrake()
{
  return mMaxBrake;
}

void Vehicle::setMaxBrake(const qreal value)
{
  mMaxBrake = value;
}

qreal Vehicle::maxAcceleration()
{
  return mMaxAcceleration;
}

void Vehicle::setWheelBase(const qreal value)
{
  mWheelBase = value;
}

qreal Vehicle::wheelBase()
{
  return mWheelBase;
}

void Vehicle::setMass(const qreal value)
{
  mMass = value;
}

qreal Vehicle::mass()
{
  return mMass;
}
void Vehicle::setMaxEngineForce(const qreal value)
{
  mMaxEngineForce = value;
  mMaxAcceleration = mMaxEngineForce;
}

qreal Vehicle::maxEngineForce()
{
  return mMaxEngineForce;
}

void Vehicle::setDragConstant(const qreal value)
{
  mDragConstant = value;
}

qreal Vehicle::dragConstant()
{
  return mDragConstant;
}

qreal Vehicle::travelDistance(bool inMiles)
{
  if (inMiles)
    return mOdometer*MILE_PER_FEET;
  else
    return mOdometer;
}

qreal Vehicle::speed( bool inMPH )
{
  if(inMPH)
    return (mVelocity.magnitude()*MPS_TO_MPH);
  else
    return mVelocity.magnitude();//*FEET_PER_METER; //in ft/s
}

void Vehicle::actuators(qreal &acl, qreal &brake, qreal &steer )
{
  acl = mAccelVal;
  brake = mBrakeVal;
  steer = mSteerAngle;
}

void Vehicle::forces(Point &engine, Point &brake, Point &friction, Point &drag)
{
  engine = mEngineForce;
  brake = mBrakeForce;
  friction = mFrictionForce;
  drag = mDragForce;
}

// Driver controls. These functions will act on vehicle
// vectors based on behaviors of the driver.
// They are not descrete SET functions like the above.
void Vehicle::setDriver(Driver *driver)
{
  mDriver = driver;
}


Driver* Vehicle::driver()
{
  return mDriver;
}

// Represents steering wheel. 0 = no adjustment,
// -1 = steer max left, 1 = steer max right.
// Changes vehicle direction.
//
//   . 0 .
// -1     1
void Vehicle::steer(qreal value)
{
  if (value<-1.0)
    value = -1.0;
  else if (value>1.0)
    value = 1.0;

  mSteerAngle = value;

}

// Represents brake pedal. 0=no brake, 1=full brake.
void Vehicle::brake(qreal value)
{
  if (value<0.0f)
    value = 0.0f;
  else if (value>1.0f)
    value = 1.0f;
  mBrakeVal = value;
  mBraking = (mBrakeVal > 0.0f);
  //float brakeConstant = value * mMaxAcceleration;
  //mBrakeForce =  mDirection * (-1 * brakeConstant);
}

// Represents gas pedal. 0=no gas (cruising), 1=full gas.
void Vehicle::accelerate(qreal value)
{
  if (value<0)
    value = 0;
  else if (value>1.0)
    value = 1.0;
  mAccelVal =  value;
  mEngineForce = mDirection * (value * mMaxAcceleration);
}

// List of commands issued by driver
void Vehicle::command(const DriverCommand &cmd)
{
  mCommands.push_back(cmd);
}

void Vehicle::updateForces()
{
  //mFrictionForce = mDirection * ((GRAVITY_MPS2 * mMass) * -mDragConstant);

  if(mBraking)
    mFrictionForce = mDirection * (-mMaxAcceleration);
  else
    mFrictionForce = Point();

  //float Cdrag = .5 * mDragConstant * 2.2f * 1.29f * pow(mVelocity.magnitude(),2)*4.5f;
  //mDragForce = mDirection * (Cdrag * -1.0f);
  mDragForce = Point();
}

void Vehicle::updateAcceleration()
{
  Point totalForce = mEngineForce + mFrictionForce + mDragForce;
  setAcceleration((totalForce / mMass));
/*
  qDebug() << "V=" << name()
      << "ef=" << QString::number(mEngineForce.magnitude(), 'f', 2)
      << "ff=" << QString::number(mFrictionForce.magnitude(), 'f', 2)
      << "df=" << QString::number(mDragForce.magnitude(), 'f', 2)
      << "bf=" << QString::number(mBrakeForce.magnitude(), 'f', 2)
      << "tf=" << QString::number(totalForce.magnitude(), 'f', 2)
      << "ms=" << QString::number(mMass, 'f', 2)
      << "ac=" << QString::number((totalForce / mMass).magnitude(), 'f', 2);
*/
}

void Vehicle::updateVelocity()
{
  // Note: Velocity is applied in the direction of the car, since the forces are.
  // This will prevent skidding, power sliding, etc.
  setVelocity((mDirection * mVelocity.magnitude()) + mAcceleration * World::gSimTimeStep);

  //if( < 0 && )
  // Prevent car from going backwards due to forces like braking.
  // NOTE: This will screw up external forces tho, like a truck crashing into
  // the car, if we ever do that.
  if (qAbs(mVelocity.angle(true) - mDirection.angle(true)) >= 90.0f)
    setVelocity(Point());
}

void Vehicle::updatePosition()
{
  // Velocity is already applied in the direction of the car here.
  setPosition(mPosition + mVelocity * FEET_PER_METER * World::gSimTimeStep);
}

void Vehicle::updateHeading()
{
  float turnRadius = mWheelBase / sin(mSteerAngle * M_PI / 3);
  float angularVelocity = (mVelocity.magnitude()*FEET_PER_METER) / (turnRadius);
  setDirection(mDirection.angle() + angularVelocity * World::gSimTimeStep * 180 / M_PI);
}


void Vehicle::tick()
{
  updateForces();
  updateAcceleration();
  updateVelocity();
  updateHeading();
  updatePosition();

//  mOdometer += mVelocity;
//  qDebug() << "VEHICLE:" << mName
//  << " P=" << mPosition.toString()
//  << " V=" << mVelocity
//  << " A=" << QString::number(mAcceleration.magnitude()*FTPS_TO_MPH) + "mph/s"
//  << " A=" << mAcceleration
//  << " D=" << mDirection.toString()
//  << " S=" << QString::number(speed(true)) + "mph"
//  << " T=" << QString::number(travelDistance()) + "ft"
//  << " t=" << World::gSimTime.toString("hh:mm:ss.zzz");
}
